
#include "cdrtdeapi.h"
#include "rtdeapi.h"
#include "rtdebasedefine.h"


//创建rtde
//ip--ip地址   version--协议版本
RtdeResult cd_rtde_create(RtdeHandle *rtdeHandle,const char *ip,int64_t version)
{
    RtdeResult result = RTDE_ERR_NONE;

    rtde_create_handle(rtdeHandle,ip);

    RtdeHandle rtdeHandle_m = *rtdeHandle;

    result = rtde_create(rtdeHandle_m,ip);//连接端口
    if(result != RTDE_ERR_NONE)
    {
        return result;
    }
    rtde_protocol_version_request(rtdeHandle_m,version);//请求协议版本
    if(result != RTDE_ERR_NONE)
    {
        return result;
    }
    rtde_all_recipe_get(rtdeHandle_m);//获取所有配方
    if(result != RTDE_ERR_NONE)
    {
        return result;
    }
    return result;
}

//获取SDK版本号
RtdeResult cd_rtde_get_sdk_version(uint8_t* version)
{
    RtdeResult result = RTDE_ERR_NONE;
    result = rtde_get_SDK_Version(version);
    return result;
}

//关闭rtde
RtdeResult cd_rtde_destroy(RtdeHandle rtdeHandle)
{
    RtdeResult result = RTDE_ERR_NONE;
    if(rtdeHandle == 0)
    {
        result = RTDE_ERR_UNKNOW;
        return result;
    }
    result = rtde_destroy(rtdeHandle);//关闭连接
    return result;
}

//配置输入配方
//recipeId --配方id
RtdeResult cd_rtde_input_setup(RtdeHandle rtdeHandle,char *varNames, int *recipeId)
{
    RtdeResult result = RTDE_ERR_NONE;
    if(rtdeHandle == 0)
    {
        result = RTDE_ERR_UNKNOW;
        return result;
    }
    
    int input_vars_n = 0;
    int input_vars_index[1000];
    input_vars_n =multi_find(rtdeHandle,varNames, INPUT, input_vars_index);//获取输入变量索引

    result = rtde_input_setup_send(rtdeHandle, input_vars_n, input_vars_index,recipeId);
    if(result != RTDE_ERR_NONE)
    {
        return result;
    }

    return result;
}

//配置输出配方
RtdeResult cd_rtde_output_setup(RtdeHandle rtdeHandle,char *varNames, int frequency,int *recipeId)
{
    RtdeResult result = RTDE_ERR_NONE;
    if(rtdeHandle == 0)
    {
        result = RTDE_ERR_UNKNOW;
        return result;
    }

    int output_vars_n = 0;
    int output_vars_index[1000];
    output_vars_n =multi_find(rtdeHandle,varNames, OUTPUT, output_vars_index);//获取输出变量索引

    result = rtde_output_setup_send(rtdeHandle, output_vars_n, output_vars_index, frequency,recipeId);
    if(result != RTDE_ERR_NONE)
    {
        return result;
    }
    return result;
}
    


//control --1开始    2-停止  控制输出配方 控制只记住最新一条输出配方
RtdeResult cd_rtde_output_control(RtdeHandle rtdeHandle,uint32_t recipeId,int control)
{
    RtdeResult result = RTDE_ERR_NONE;
    if(rtdeHandle == 0)
    {
        result = RTDE_ERR_UNKNOW;
        return result;
    }
    if(control == 1)
    {
        result = rtde_output_start(rtdeHandle, recipeId);//开始控制
    }
    else if(control == 2)
    {
        result = rtde_output_pause(rtdeHandle, recipeId);//停止控制
    }
    else
    {
        result = RTDE_ERR_UNKNOW;
    }
    return result;
}
    

//接收数据
RtdeResult cd_rtde_output_data_receive(RtdeHandle rtdeHandle,uint32_t recipeId,void *data)
{
    RtdeResult result = RTDE_ERR_NONE;
    if(rtdeHandle == 0)
    {
        result = RTDE_ERR_UNKNOW;
        return result;
    }
    result = rtde_ouput_data_receive(rtdeHandle, recipeId, data);
    return result;
}

//设置速度百分比
RtdeResult cd_rtde_percentvelocity_set(RtdeHandle rtdeHandle,double percentVelocity)
{
    RtdeResult result = RTDE_ERR_NONE;
    if(rtdeHandle == 0)
    {
        result = RTDE_ERR_UNKNOW;
        return result;
    }
    result = rtde_set_percentvelocity(rtdeHandle, percentVelocity);
    return result;
}

//方法
/*
输入参数：
q_robot：关节给定位置（°）
q_exj：外部轴给定位置（°或mm）
cutoff_freq：滤波截止频率（rad/s）
返回值：
0-正常；其他-错误
*/
RtdeResult cd_rtde_servoj(RtdeHandle rtdeHandle,double jointPose[6],uint8_t exjNum,double *exjPose,double cutoffFreq)
{
    RtdeResult result = RTDE_ERR_NONE;
    if(rtdeHandle == 0)
    {
        result = RTDE_ERR_UNKNOW;
        return result;
    }

    int methodCode = find_method_index(rtdeHandle,"servoJ");

    result = crtde_servoj(rtdeHandle, methodCode,cutoffFreq,jointPose,exjNum,exjPose);
    return result;
}



/*
用户函数，停止关节透传运动
输入参数：
rtdeHandle：rtde句柄
deceleration：减速比
sync：关节和外部轴是否同步停止（true: 同步，false: 独立）
返回值：
0-正常；其他-错误
*/
RtdeResult cd_rtde_servo_stop(RtdeHandle rtdeHandle,double deceleration)
{
    RtdeResult result = RTDE_ERR_NONE;
    if(rtdeHandle == 0)
    {
        result = RTDE_ERR_UNKNOW;
        return result;
    }

    int methodCode = find_method_index(rtdeHandle,"servoStop");

    result = crtde_servo_stop(rtdeHandle, methodCode,deceleration);
    return result;
}


/*
用户函数，复位错误
输入参数：
rtdeHandle：rtde句柄
返回值：
0-正常；其他-错误
*/
RtdeResult cd_rtde_reseterror(RtdeHandle rtdeHandle)
{
    RtdeResult result = RTDE_ERR_NONE;
    if(rtdeHandle == 0)
    {
        result = RTDE_ERR_UNKNOW;
        return result;
    }

    int methodCode = find_method_index(rtdeHandle,"resetError");

    result = crtde_resterror(rtdeHandle, methodCode);
    return result;
}